#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "camellia.h"
#include <time.h>
#include <inttypes.h>
int64_t timespecDiff(struct timespec *timeA_p, struct timespec *timeB_p)
{
	  return ((timeA_p->tv_sec * 1000000000) + timeA_p->tv_nsec) -
	             ((timeB_p->tv_sec * 1000000000) + timeB_p->tv_nsec);
}
int main(int argc, char* argv[])
{
    CamImage image, Y1, Y2;
    CamKeypoints points1, points2;
    int i, nbMatches;
    int dist1, dist2, color;
    CamKeypoint *best;
    float ratio = 2.0f/3;
    printf("Feature point detection on %s :\n",argv[1]);
    image.imageData = NULL;
    camLoadBMP(&image, argv[1]);
    Y1.imageData = NULL;
    camRGB2Y(&image, &Y1);
    camAllocateKeypoints(&points1, 100000);
    camAllocateKeypoints(&points2, 100000);
    camAllocateImage(&Y2, (((int)(Y1.width*ratio))/8)*8, (((int)(Y1.height*ratio))/8)*8, CAM_DEPTH_8U);
    camScale(&Y1, &Y2);
    camSavePGM(&Y2, argv[2]);

    camFastHessianDetector(&Y1, &points1, 500, 0);
    camFastHessianDetector(&Y2, &points2, 500, 0);
	
    nbMatches = 0;
    
    for (i = 0; i < points1.nbPoints; i++) {
        best = camFindKeypoint(points1.keypoint[i], &points2, &dist1, &dist2);
	if (dist1 < 0.7 * dist2) {
	    if (fabs(ratio * points1.keypoint[i]->x - best->x) > 10) color = CAM_RGB(0, 0 , 255); 
	    else if (fabs(ratio * points1.keypoint[i]->y - best->y) > 10) color = CAM_RGB(0, 0 , 255); 
	    else {
		color = CAM_RGB(0, 255, 0);
		nbMatches++;
	    }
	} else color = CAM_RGB(255, 0, 0);
	camDrawKeypoint(points1.keypoint[i], &image, color);
	printf("x=%d y=%d mark=%d scale=%d meaning=%lf\n", points1.keypoint[i]->x, points1.keypoint[i]->y, points1.keypoint[i]->value, points1.keypoint[i]->scale, ((double)dist1) / dist2);
    }
    printf("# points found on 1st image: %d\n", points1.nbPoints);
    printf("# points found on 2nd image: %d\n", points2.nbPoints);
    printf("# points matched: %d\n", nbMatches);
    
    camSaveBMP(&image, argv[3]);
    camDeallocateImage(&image);
    camDeallocateImage(&Y1);
    camDeallocateImage(&Y2);
    camFreeKeypoints(&points1);
    camFreeKeypoints(&points2);
}
